/**
  ******************************************************************************
  * @file           : vision_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/28
  ******************************************************************************
  */



#include "vision_task.h"


uint8_t VisionControlFlag = 0; //识别目标标志位
uint8_t VisionControlFlagLast = 0;
uint8_t vision_cal_completed = 0; //计算完成标志位

int32_t temporary_pause_cnt = 0; //视觉暂留时间
uint8_t is_temporary_pause = 0;  //视觉暂留标志位

uint8_t target_change = 0;       //目标是否切换
uint32_t target_recognition_cnt = 0; //识别到该目标的计数
uint8_t armor_number = 0;
uint8_t armor_number_last = 0;
uint8_t armor_number_no_zero = 0;

pose_cal poseCal={};

float enemy_distance = 0;

extern rc_device_t g_rc;
extern VisionReceiveDataStruct VisionReceiveData;
extern StampDataStruct TimeStampedData;
extern chassis_info chassisInfo;
extern VisionDataStruct VisionReceive;
extern twins_receive_info twins_receive;
extern twins_tran_info twins_tran;
extern osSemaphoreId_t VisionCalBinarySemHandle;


extern float pitch_set_gyro;
extern float yaw_set_gyro;
extern float VisionChangeYaw;
extern float VisionChangePitch;
extern uint8_t enemy_aim_enable[12];


float large_yaw_theta = 0; //弧度

float deg2rad(float deg){
    return(deg / 360.f * 2 * PI);
}

float float2rad(float float_){
    return(float_ * 2.f * PI);
}

float rad2deg(float rad){
    return(rad / (2 * PI) * 360);
}


Point3fStruct robot_center_to_armor;
//世界坐标系作为中间媒介
//将相机看作与枪管重合，即z_offset=0
//为简化计算，pitch的值直接相等
//其中，角度的单位为deg，长度的单位为mm，
void another_gimbal_yaw_pitch_cal(float yaw, float pitch, float armor_x, float armor_y, float armor_z, float &another_yaw, float &another_pitch, float &chassis_target_yaw){
    //以下为世界坐标系
    Point3fStruct armor_coo;
    Point3fStruct this_camera_to_armor;
    Point3fStruct this_rotation_center_to_this_camera;
    Point3fStruct another_rotation_center_to_this_rotation_center;
    Point3fStruct another_rotation_center_to_armor;

    float armor_coo_pitch_rotated[3];
    float armor_coo_camera[3];
    float camera_rotation_matrix_pitch[9];
    float camera_rotation_matrix_yaw[9];
    float armor_coo_world[3];

    float this_gimbal_yaw = deg2rad(yaw);
    float this_gimbal_pitch = deg2rad(pitch);
    large_yaw_theta = float2rad(chassisInfo.yaw_angle);

    //先改为车体坐标系，发送的时候再改回去，方便阅读
    armor_coo.x = armor_z;
    armor_coo.y = -armor_x;
    armor_coo.z = -armor_y;

    armor_coo_camera[0] = armor_coo.x;
    armor_coo_camera[1] = armor_coo.y;
    armor_coo_camera[2] = armor_coo.z;

    mat_rot_y(this_gimbal_pitch, camera_rotation_matrix_pitch);
    matXvec(camera_rotation_matrix_pitch, armor_coo_camera, armor_coo_pitch_rotated);
    rotate_z_rotation_matrix(this_gimbal_yaw, camera_rotation_matrix_yaw);
    matXvec(camera_rotation_matrix_yaw, armor_coo_pitch_rotated, armor_coo_world);

    this_camera_to_armor.x = armor_coo_world[0];
    this_camera_to_armor.y = armor_coo_world[1];
    this_camera_to_armor.z = armor_coo_world[2];

    this_rotation_center_to_this_camera.x = cos(this_gimbal_yaw) * CAMERA_TO_ROTATION_CENTER_X * cos(this_gimbal_pitch);
    this_rotation_center_to_this_camera.y = sin(this_gimbal_yaw) * CAMERA_TO_ROTATION_CENTER_X * cos(this_gimbal_pitch);
    this_rotation_center_to_this_camera.z = -sin(this_gimbal_pitch) * CAMERA_TO_ROTATION_CENTER_X;

    another_rotation_center_to_this_rotation_center.x = -sin(large_yaw_theta) * DISTANCE_TWO_GIMBAL;
    another_rotation_center_to_this_rotation_center.y = cos(large_yaw_theta) * DISTANCE_TWO_GIMBAL;
    another_rotation_center_to_this_rotation_center.z = 0;

    //这里用作计算大yaw旋转的角度
    robot_center_to_armor.x = another_rotation_center_to_this_rotation_center.x/2.f+this_rotation_center_to_this_camera.x+this_camera_to_armor.x;
    robot_center_to_armor.y = another_rotation_center_to_this_rotation_center.y/2.f+this_rotation_center_to_this_camera.y+this_camera_to_armor.y;
    robot_center_to_armor.z = another_rotation_center_to_this_rotation_center.z/2.f+this_rotation_center_to_this_camera.z+this_camera_to_armor.z;

    enemy_distance = pow(pow(robot_center_to_armor.x / 1000, 2) + pow(robot_center_to_armor.y / 1000, 2), 0.5);

    another_rotation_center_to_armor.x = another_rotation_center_to_this_rotation_center.x+this_rotation_center_to_this_camera.x+this_camera_to_armor.x;
    another_rotation_center_to_armor.y = another_rotation_center_to_this_rotation_center.y+this_rotation_center_to_this_camera.y+this_camera_to_armor.y;
    another_rotation_center_to_armor.z = another_rotation_center_to_this_rotation_center.z+this_rotation_center_to_this_camera.x+this_camera_to_armor.z;

    another_pitch = pitch;
    float another_yaw_tmp = atan2(another_rotation_center_to_armor.y, another_rotation_center_to_armor.x) / (2.f * PI) * 360.f;
    another_yaw = another_yaw_tmp < 0 ? another_yaw_tmp + 360 : another_yaw_tmp; //归到0-360

    float chassis_target_yaw_tmp = atan2(robot_center_to_armor.y, robot_center_to_armor.x) / (2.f * PI) * 360.f;
    chassis_target_yaw = chassis_target_yaw_tmp < 0 ? chassis_target_yaw_tmp + 360 : chassis_target_yaw_tmp; //归到0-360
}


void vision_task(void* argument) {
    VisionInit();
    TimeStampedData.time_stamp =0;//Init time_stamp = 0
    osStatus_t status = osOK;
    for (;;) {
        status = osSemaphoreAcquire(VisionCalBinarySemHandle, 100);
        if(status != osOK){
            VisionControlFlagLast = VisionControlFlag;
            VisionControlFlag = 0;
            vision_cal_completed = 0;
            VisionReceiveData.change_pitch = 0;
            VisionReceiveData.change_yaw = 0;
            temporary_pause_cnt = 0;
            is_temporary_pause = 0;
            target_recognition_cnt = 0;
            continue;
        }
        VisionControlFlagLast = VisionControlFlag;
        AimAutoData.AutoAim = twins_receive.auto_aim; //由右头决策控制
        //如果视觉丢数据、未识别到目标或已经关控时，关闭视觉控制
        if(AimAutoData.AutoAim && !AimAutoData.AutoAimStatus){
            if((fabs(VisionReceive.vision_data.armor_axis_z < 1e-5) && !VisionReceive.vision_data.firing_rate) || g_rc.is_lost == 1){
                vision_cal_completed = 0;
                VisionControlFlag = 0;
                VisionReceiveData.change_pitch = 0;
                VisionReceiveData.change_yaw = 0;
            }
                //如果视觉识别到目标，并且开启了自瞄
            else{
                VisionControlFlag = 1;
            }
        }else{
            VisionControlFlag = 0;
            vision_cal_completed = 0;
        }

        /********************* 目标识别计数 *******************/
        armor_number_no_zero = VisionReceive.vision_data.ser_z != 0 && VisionReceive.vision_data.ser_z <= 12 ? VisionReceive.vision_data.ser_z : armor_number_no_zero;
        armor_number_last = armor_number;
        armor_number = VisionReceive.vision_data.ser_z != 100 ? VisionReceive.vision_data.ser_z : armor_number;
        if(armor_number != armor_number_last){
            target_change = 1;
            target_recognition_cnt = 0;
        }else{
            target_change = 0;
            target_recognition_cnt++;
        }

        if (VisionControlFlag && enemy_aim_enable[armor_number] == 1){
//        if (VisionControlFlag){
            if(AimAutoData.AutoAimStatus == 0){
                AimAuto();
                another_gimbal_yaw_pitch_cal(yaw_set_gyro * 360.f, pitch_set_gyro * 180.f,
                                             VisionReceive.vision_data.armor_axis_x, VisionReceive.vision_data.armor_axis_y, VisionReceive.vision_data.armor_axis_z,
                                             poseCal.gimbal_yaw, poseCal.gimbal_pitch, poseCal.chassis_yaw);
                vision_cal_completed = 1;
            }
        }else{
            VisionChangeYaw = 0;
            VisionChangePitch = 0;
        }

        /********************* 视觉暂留 *******************/
        temporary_pause_cnt = temporary_pause_cnt <= 0 ? 0 : temporary_pause_cnt - 1;
        if(VisionControlFlagLast == 1 && VisionControlFlag == 0 && enemy_aim_enable[armor_number_no_zero] == 1){
            temporary_pause_cnt = TEMPORARY_PAUSE_TIME;
        }else if(VisionControlFlag == 1 && enemy_aim_enable[armor_number_no_zero] == 1){
            temporary_pause_cnt = 0;
        }
        is_temporary_pause = temporary_pause_cnt > 0 ? 1 : 0;

    }
}